
# 1.导包；
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from base_interfaces_demo.msg import SensorData
 
class DataReceiver(Node):
    def __init__(self):
        super().__init__('data_receiver')
        # 创建订阅者，订阅 '/sensor_data' 话题
        self.subscription = self.create_subscription(
            SensorData,
            '/sensor_data',
            self.listener_callback,
            10  # QoS（服务质量）设置
        )
        # 创建发布者，用于发布异常信息
        self.publisher = self.create_publisher(String, '/exception_data', 10)

    def listener_callback(self, msg):
        try:
            # 在此处添加数据验证逻辑
            if not self.validate_data(msg):
                raise ValueError("收到的数据格式异常")
            
            # 正常处理数据
            self.get_logger().info(f"收到数据: {msg}")
        
        except Exception as e:
            # 捕获异常并发布异常信息
            self.publish_exception(str(e))

    def validate_data(self, msg):
        # 数据验证函数，根据实际需求实现
        # 例如，检查温度值是否在合理范围内
        if msg.temperature < -50 or msg.temperature > 100:
            return False
        if msg.humidity < 0 or msg.humidity > 100:
            return False
        return True

    def publish_exception(self, exception_message):
        # 创建异常消息并发布
        exception_msg = String()
        exception_msg.data = exception_message
        self.publisher.publish(exception_msg)
        self.get_logger().warn(f"已发布异常信息: {exception_message}")

def main(args=None):
    rclpy.init(args=args)  # 初始化 ROS 2 系统
    data_receiver = DataReceiver()  # 创建节点实例
    rclpy.spin(data_receiver)  # 启动事件循环，处理回调
    data_receiver.destroy_node()  # 销毁节点
    rclpy.shutdown()  # 关闭 ROS 2 系统

if __name__ == '__main__':
    main()